#include "move.h"
#include "delay.h"
#include "pid/pid_ctrl.h"
#include "jy61p/jy61p.h"
#include "OLED/oled.h"
#include "stdio.h"
#include "line_flw/line_flw.h"
#include "BSP/bsp.h"

#define __MAX_SPEED__       (0.42f)
#define __MAX_SPEED__SLOW   (0.25f)
#define __MAX_TRUNSPEED__SLOW   (0.25f)

char ob[20];
volatile move_status_t move_status;

// 全局PID控制器实例
static pid_ctrl_t pid;

// 循迹PID实例
static pid_ctrl_t lf_pid;

// 初始化直线行驶功能
void move_init() 
{
    // 初始化PID控制器
    pid_ctrl_init(&pid, 0.0042f, 0.00012f, 0.00005f, 0.01f);  // PID参数需要根据实际情况调整
    pid_ctrl_set_target(&pid, my_jy61p.yaw);  // 目标角度为0度，保持直线行驶
}

void move_slow_init() 
{
    // 初始化PID控制器
    pid_ctrl_init(&pid, 0.0032f, 0.0005f, 0.0f, 0.01f);  // PID参数需要根据实际情况调整
    pid_ctrl_set_target(&pid, my_jy61p.yaw);  // 目标角度为0度，保持直线行驶
}

// 直线行驶控制
void move_straight_proccess(motor_t* motor_1, motor_t* motor_2, float base_speed) 
{
    // 使用PID控制器调整电机速度，保持直线行驶
    if (base_speed > 0) 
    {
        // 获取当前yaw角度作为反馈值
        float yaw = my_jy61p.yaw;
        
        // 计算PID输出
        float error = pid.target - yaw;
        if (error > 180)
        {
            error -= 360;
        }
        else if (error < -180)
        {
            error += 360;
        }
        float pid_output = pid_ctrl_update(&pid, error);

        // sprintf(ob, "y: %.2f  p: %.2f", my_jy61p.yaw, pid_output);
        // OLED_ShowString(0, 0, ob, 8);
        
        // 根据PID输出调整两个电机的速度
        float left_speed = base_speed + pid_output;
        float right_speed = base_speed - pid_output;

        // sprintf(ob, "l: %.2f  r: %.2f", left_speed, right_speed);
        // OLED_ShowString(0, 4, ob, 8);
        
        // 限制速度在0-1范围内
        if (left_speed > __MAX_SPEED__) left_speed = __MAX_SPEED__;
        if (left_speed < 0.0f) left_speed = 0.0f;
        if (right_speed > __MAX_SPEED__) right_speed = __MAX_SPEED__;
        if (right_speed < 0.0f) right_speed = 0.0f;
        
        // 设置电机速度
        motor_set_speed(motor_1, left_speed);
        motor_set_speed(motor_2, right_speed);
    } 
    else 
    {
        motor_set_speed(motor_1, 0.0f);
        motor_set_speed(motor_2, 0.0f);
    }
}

void turn_left()
{
    float target = pid.target + 90;
    while (target - 360 > 0) 
    {
        target -= 360;
    }
    pid_ctrl_set_target(&pid, target);
}

void turn_right()
{
    float target = pid.target - 90;
    while (target + 360 < 0) 
    {
        target += 360;
    }
    pid_ctrl_set_target(&pid, target);
}

void line_move_init() 
{
    // 初始化PID控制器
    pid_ctrl_init(&lf_pid, 0.42f, 0.0045f, 0.001f, 0.01f);  // PID参数需要根据实际情况调整
    pid_ctrl_set_target(&lf_pid, 0.0f);  // 目标角度为0度，保持直线行驶
}

void line_move_slow_init() 
{
    // 初始化PID控制器
    pid_ctrl_init(&lf_pid, 0.25f, 0.0045f, 0.001f, 0.01f);  // PID参数需要根据实际情况调整
    pid_ctrl_set_target(&lf_pid, 0.0f);  // 目标角度为0度，保持直线行驶
}

static const float sensor_weights[12] = {
    -1.0, -0.818, -0.636, -0.454, -0.272, -0.090, 
    0.090, 0.272, 0.454, 0.636, 0.818, 1.0
};

// 计算线误差
float line_error(uint16_t states)
{
    // 初始化加权总和和活动计数
    float weighted_sum = 0;
    int active_count = 0;
  
    // 遍历传感器状态
    for (int i = 0; i < 12; i++) 
    {
        // 如果传感器状态为1
        if ((states & (1 << i)) != 0) 
        {
            // 加上传感器权重
            weighted_sum += sensor_weights[i];
            // 活动计数加1
            active_count++;
        }
    }

    // 计算误差
    float error = weighted_sum / active_count;

    // 如果活动计数大于等于4
    if (active_count >= 4)
    {
        // 设置移动状态为转弯
        move_status = TURN;
        // 初始化移动
        move_init();
        turn_left();
        return 0;
        // 返回0
    }

    return error;
    // 返回误差
}

float line_slow_error(uint16_t states)
{
    // 初始化加权总和和活动计数
    float weighted_sum = 0;
    int active_count = 0;
  
    // 遍历传感器状态
    for (int i = 0; i < 12; i++) 
    {
        // 如果传感器状态为1
        if ((states & (1 << i)) != 0) 
        {
            // 加上传感器权重
            weighted_sum += sensor_weights[i];
            // 活动计数加1
            active_count++;
        }
    }

    // 计算误差
    float error = weighted_sum / active_count;

    // 如果活动计数大于等于4
    if (active_count >= 4)
    {
        // 设置移动状态为转弯
        move_status = TURN;
        // 初始化移动
        move_slow_init();
        turn_left();
        return 0;
        // 返回0
    }

    return error;
    // 返回误差
}

void line_straight(motor_t* motor_1, motor_t* motor_2, float base_speed)
{
    // 获取传感器状态
    uint16_t s = line_flw_getPinsstate(&my_line_flw);

    // 计算线误差
    float error = line_error(s);
    // sprintf(ob, "y: %.2f", error);
    // OLED_ShowString(0, 0, (uint8_t*)ob, 8);

    // 计算PID输出
    float pid_output = pid_ctrl_update(&lf_pid, error);

    // 计算左右电机速度
    float left_speed = base_speed + pid_output;
    float right_speed = base_speed - pid_output;

    // sprintf(ob, "l: %.2f  r: %.2f", left_speed, right_speed);
    // OLED_ShowString(0, 4, ob, 8);
    
    // 限制速度在0-1范围内
    if (left_speed > __MAX_SPEED__) left_speed = __MAX_SPEED__;
    if (left_speed < 0.0f) left_speed = 0.0f;
    if (right_speed > __MAX_SPEED__) right_speed = __MAX_SPEED__;
    if (right_speed < 0.0f) right_speed = 0.0f;
    
    // 设置电机速度
    motor_set_speed(motor_1, right_speed);
    motor_set_speed(motor_2, left_speed);
}

void line_straight_slow(motor_t* motor_1, motor_t* motor_2, float base_speed)
{
    // 获取传感器状态
    uint16_t s = line_flw_getPinsstate(&my_line_flw);

    // 计算线误差
    float error = line_slow_error(s);
    // sprintf(ob, "y: %.2f", error);
    // OLED_ShowString(0, 0, (uint8_t*)ob, 8);

    // 计算PID输出
    float pid_output = pid_ctrl_update(&lf_pid, error);

    // 计算左右电机速度
    float left_speed = base_speed + pid_output;
    float right_speed = base_speed - pid_output;

    // sprintf(ob, "l: %.2f  r: %.2f", left_speed, right_speed);
    // OLED_ShowString(0, 4, ob, 8);
    
    // 限制速度在0-1范围内
    if (left_speed > __MAX_SPEED__SLOW) left_speed = __MAX_SPEED__SLOW;
    if (left_speed < 0.0f) left_speed = 0.0f;
    if (right_speed > __MAX_SPEED__SLOW) right_speed = __MAX_SPEED__SLOW;
    if (right_speed < 0.0f) right_speed = 0.0f;
    
    // 设置电机速度
    motor_set_speed(motor_1, right_speed);
    motor_set_speed(motor_2, left_speed);
}

void turn_line_flw(motor_t* motor_1, motor_t* motor_2)
{
    motor_set_speed(motor_1, 0.28);
    motor_set_speed(motor_2, 0.057);

    delay_ms(666);

    move_status = STRAIGHT;
}

void turn_imu(motor_t* motor_1, motor_t* motor_2, float base_speed)
{
    // 获取当前yaw角度作为反馈值
    float yaw = my_jy61p.yaw;

    // 计算PID输出
    float error = pid.target - yaw;
    if (error > 180)
    {
        error -= 360;
    }
    else if (error < -180)
    {
        error += 360;
    }

    if (error <= 4)
    {
        move_status = STRAIGHT;
        return;
    }

    float pid_output = pid_ctrl_update(&pid, error);

    // sprintf(ob, "y: %.2f  e: %.2f", my_jy61p.yaw, error);
    // OLED_ShowString(0, 0, ob, 8);
    
    // 根据PID输出调整两个电机的速度
    float left_speed = base_speed + pid_output;
    float right_speed = base_speed - pid_output;

    // sprintf(ob, "l: %.2f  r: %.2f", left_speed, right_speed);
    // OLED_ShowString(0, 4, ob, 8);
    
    // 限制速度在0-1范围内
    if (left_speed > __MAX_SPEED__) left_speed = __MAX_SPEED__;
    if (left_speed < 0.0f) left_speed = 0.0f;
    if (right_speed > __MAX_SPEED__) right_speed = __MAX_SPEED__;
    if (right_speed < 0.0f) right_speed = 0.0f;
    
    // 设置电机速度
    motor_set_speed(motor_1, left_speed);
    motor_set_speed(motor_2, right_speed);
}

void turn_imu_slow(motor_t* motor_1, motor_t* motor_2, float base_speed)
{
    // 获取当前yaw角度作为反馈值
    float yaw = my_jy61p.yaw;

    // 计算PID输出
    float error = pid.target - yaw;
    if (error > 180)
    {
        error -= 360;
    }
    else if (error < -180)
    {
        error += 360;
    }

    if (error <= 4)
    {
        move_status = STRAIGHT_SLOW;
        return;
    }

    float pid_output = pid_ctrl_update(&pid, error);

    // sprintf(ob, "y: %.2f  e: %.2f", my_jy61p.yaw, error);
    // OLED_ShowString(0, 0, ob, 8);
    
    // 根据PID输出调整两个电机的速度
    float left_speed = base_speed + pid_output;
    float right_speed = base_speed - pid_output;

    // sprintf(ob, "l: %.2f  r: %.2f", left_speed, right_speed);
    // OLED_ShowString(0, 4, ob, 8);
    
    // 限制速度在0-1范围内
    if (left_speed > __MAX_TRUNSPEED__SLOW) left_speed = __MAX_TRUNSPEED__SLOW;
    if (left_speed < 0.0f) left_speed = 0.0f;
    if (right_speed > __MAX_TRUNSPEED__SLOW) right_speed = __MAX_TRUNSPEED__SLOW;
    if (right_speed < 0.0f) right_speed = 0.0f;
    
    // 设置电机速度
    motor_set_speed(motor_1, left_speed);
    motor_set_speed(motor_2, right_speed);
}
